/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_MOCAPMARKERSENSORMEASUREMENT_H_
#define __MMM_MOCAPMARKERSENSORMEASUREMENT_H_

#include "../../InterpolatableSensorMeasurement.h"
#include "../../SensorMeasurement.h"
#include "../../MarkerData.h"

#include <Eigen/Core>

namespace MMM
{
class MoCapMarkerSensorMeasurement;

typedef boost::shared_ptr<MoCapMarkerSensorMeasurement> MoCapMarkerSensorMeasurementPtr;

class MoCapMarkerSensorMeasurement : public InterpolatableSensorMeasurement<MoCapMarkerSensorMeasurement>, SMCloneable<MoCapMarkerSensorMeasurement>
{

public:
    MoCapMarkerSensorMeasurement(float timestep, const std::map<std::string, Eigen::Vector3f> &marker, const std::vector<Eigen::Vector3f> &unlabeledMarker, SensorMeasurementType type = SensorMeasurementType::MEASURED);

    MoCapMarkerSensorMeasurement(float timestep, MarkerDataPtr markerData, SensorMeasurementType type = SensorMeasurementType::MEASURED);

    SensorMeasurementPtr clone();

    bool equals(SensorMeasurementPtr sensorMeasurement);

    MoCapMarkerSensorMeasurementPtr clone(float newTimestep);

    bool hasMarker(const std::string &label);

    Eigen::Vector3f getLabeledMarker(const std::string &label);

    std::map<std::string, Eigen::Vector3f> getLabeledMarker();

    std::vector<Eigen::Vector3f> getUnlabeledMarker();

protected:
    MoCapMarkerSensorMeasurementPtr interpolate(MoCapMarkerSensorMeasurementPtr other, float timestep, SensorMeasurementType type);

    void appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode);

private:
    MarkerDataPtr markerData;

};
}
#endif // __MMM_MoCapMarkerSENSORMEASUREMENT_H_
